#include <server/gdbPacket.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

gdbPacket::gdbPacket(uint32_t bufsz)
{
    _bufsz = 0;
    _dlen = 0;
    _sdlen = 0;
    _sbufsz = 0;

    _buf = (char*)malloc(bufsz * 3 + 1);
    if (_buf)
    {
        _bufsz = bufsz;
        _sbuf = _buf + bufsz;
        _sbufsz = bufsz * 2;
    }
}

gdbPacket::~gdbPacket()
{
    free(_buf);
}

char* gdbPacket::request()
{
    return _buf;
}

char *gdbPacket::data(uint32_t pos)
{
    return &_buf[pos];
}

uint32_t gdbPacket::bufsz()
{
    return _bufsz;
}

uint32_t gdbPacket::length()
{
    return _dlen;
}

void gdbPacket::resetSendBuf()
{
    _sbuf[0] = 0;
    _sdlen = 1;
}

void gdbPacket::resetRecvBuf()
{
    _buf[0] = 0;
    _dlen = 0;
}

bool gdbPacket::putRecvBuf(char c)
{
    if (_dlen < _bufsz)
    {
        _buf[_dlen] = c;
        _dlen ++;
        return true;
    }

    return false;
}

bool gdbPacket::check(uint32_t dlen)
{
    uint8_t chk;
    long rchk;

    if (dlen < 3 || dlen > _bufsz)
        return false;

    if (_buf[dlen - 3] != '#')
        return false;

    chk = checkSum(_buf, dlen - 3);
    rchk = strtol(&_buf[dlen - 2], NULL, 16);
    if (chk != rchk)
        return false;

    _dlen = dlen - 3;
    _buf[_dlen] = 0;

    return true;
}

bool gdbPacket::append(char c)
{
    if (_sdlen >= _sbufsz)
        return false;

    _sbuf[_sdlen] = c;
    _sdlen ++;

    return true;
}

bool gdbPacket::append(const char *str, int len)
{
    bool ret;

    if (len == -1)
        len = strlen(str);

    for (int i = 0; i < len; i ++)
    {
        ret = append(str[i]);
        if (!ret)
            break;
    }

    return ret;
}

bool gdbPacket::append(uint8_t d)
{
    char hex[3];

    sprintf(hex, "%02x", d);

    return append(hex, 2);
}

bool gdbPacket::append(uint8_t *d, uint32_t len)
{
    bool ret;

    for (int i = 0; i < len; i ++)
    {
        ret = append(d[i]);
        if (!ret)
            break;
    }

    return ret;
}

bool gdbPacket::makeResponse()
{
    uint8_t sum;

    if (_sdlen > (_sbufsz - 3))
    {
        _sbuf[0] = 0;
        return false;
    }

    _sbuf[0] = '$';

    sum = checkSum(&_sbuf[1], _sdlen - 1);
    append('#');
    append(sum);

    _sbuf[_sdlen] = 0; //actual size = _sbufsz + 1

    return true;
}

char* gdbPacket::response(uint32_t *len)
{
    if (len)
    {
        *len = _sdlen;
        if (_sbuf[0] == 0)
            *len = 0;
    }

    return _sbuf;
}

uint32_t gdbPacket::addPos(uint32_t len)
{
    _dlen += len;

    return _dlen;
}

uint8_t gdbPacket::checkSum(char *d, int len)
{
    uint8_t s = 0;
    int i;

    for (i = 0; i < len; i ++)
    {
        s += d[i];
    }

    return s;
}

char gdbPacket::getCmd()
{
    return _buf[0];
}

bool gdbPacket::replyStop()
{
    append("T02", 3);
    return makeResponse();
}

bool gdbPacket::replyEmpty()
{
    return makeResponse();
}

bool gdbPacket::replyOk()
{
    append("OK", 2);
    return makeResponse();
}

bool gdbPacket::replySignal(uint8_t sig)
{
    append('S');
    append(sig);
    return makeResponse();
}

bool gdbPacket::replyError(uint8_t eno)
{
    append('E');
    append(eno);
    return makeResponse();   
}
